#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#include <unistd.h>
#include <fcntl.h>
#include "dev_spi_flash_sfud.h"
typedef void (*app_func)(void);

static void jump_to_app(uint32_t app_addr)
{
    app_func jump_func;
    
    /* 关闭中断 */
    rt_hw_interrupt_disable();
    
    /* 设置堆栈指针 */
    __set_MSP(*(volatile uint32_t*)app_addr);
    
    /* 跳转到应用程序 */
    jump_func = (app_func)(*(volatile uint32_t*)(app_addr + 4));
    jump_func();
}


#include "w25q128jv.h"
#include "octospi.h"
#define OSPI_DEVICE_NAME "ospi10"
#define OSPI_FLASH_DEVICE_NAME "W25Q64Q"

#define QSPI_OFFSET (4*1024*1024) // 4MB偏移量
#define QSPI_BASE_ADDR 0x90000000  // QSPI内存映射基地址
#define APP_ADDRESS (QSPI_BASE_ADDR + QSPI_OFFSET)

char buffer[1024]; // 读写缓冲区
char buffer2[1024]; // 比较缓冲区
int run_in_qspi(int argc, char *argv[])
{    
    int fd_src, fd_dst;
    int size;
    
    if(argc != 2) {
        rt_kprintf("Usage: run_in_qspi <src>\n");
        rt_kprintf("Example: run_in_qspi spi\n");
        return -1;
    }

    // 1. 从源文件系统打开bin文件
    if(rt_strcmp(argv[1], "spi") == 0)
        fd_src = open("/spi/STM32H7B0VBT6-QSPI.bin", O_RDONLY);
    else if(rt_strcmp(argv[1], "qspi") == 0)
        fd_src = open("/qspi/STM32H7B0VBT6-QSPI.bin", O_RDONLY);
    else if(rt_strcmp(argv[1], "sdcard") == 0)
        fd_src = open("/sdcard/STM32H7B0VBT6-QSPI.bin", O_RDONLY);    
    else
        return -1;

    if (fd_src < 0) {
        rt_kprintf("Open source file failed!\n");
        return -1;
    }

    // 2. 使用sfud直接写入QSPI Flash
    rt_spi_flash_device_t rtt_dev = rt_sfud_flash_probe(OSPI_FLASH_DEVICE_NAME, OSPI_DEVICE_NAME);
    sfud_flash_t sfud_dev = (sfud_flash_t)rtt_dev->user_data;

    rt_kprintf("Erasing QSPI Flash at 0x%08X...\n", QSPI_OFFSET);
    if (sfud_erase(sfud_dev, QSPI_OFFSET, 0x10000) != RT_EOK) {
        rt_kprintf("Erase failed!\n");
        goto _exit;
    }

    // 写入数据到QSPI Flash
    rt_kprintf("Writing to QSPI Flash...\n");
    uint32_t offset= QSPI_OFFSET;
    while ((size = read(fd_src, buffer, sizeof(buffer))) > 0) {
        if (sfud_write(sfud_dev, offset, size, buffer) != RT_EOK) {
            rt_kprintf("Write failed!\n");
            goto _exit;
        }
        sfud_read(sfud_dev, offset, size, buffer2);
        if (rt_memcmp(buffer, buffer2, size)!= 0) {
            rt_kprintf("Compare failed!\n");
            goto _exit;
        }
        offset += size;
    }

    OCTOSPI1->CR = OCTOSPI1->CR & 0x1ffffffe; // Exit mem map, Disable Qspi
    MX_OCTOSPI1_Init();
    uint8_t id[2];
    OSPI_W25Q128JV_DeviceID(id);
    OSPI_W25Q128JV_Reset();
    OSPI_W25Q128JV_EnableMemoryMappedMode();

    // 3. 直接从内存映射地址执行 (无需拷贝)
    rt_kprintf("Jumping to app at 0x%08X...\n", APP_ADDRESS);
    jump_to_app(APP_ADDRESS);

_exit:
    close(fd_src);
    return -1; 
}

/*注意, 在menuconfig中要取消BSP_OSPI_USING_SOFTCS, 否则无法正常运行XIP*/
MSH_CMD_EXPORT(run_in_qspi, run in qspi);